#include <iostream>
#include "read_data.h"
#include "imu_integration.h"
#include "optimizer.h"

using namespace std;

int main() {  
	double acc_noise_std = 0.1;
	double init_bias = 0.0;
	std::vector<double> timestamps;
	std::vector<Eigen::Vector3d> imu_data;  
	std::vector<Eigen::Vector3d> odometry_data;
	std::vector<Eigen::Vector3d> vel_data;  
	std::vector<Eigen::Vector3d> pose_data;
	std::vector<Eigen::Vector3d> gps_data;
	read_simulate(init_bias, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);

	ImuIntegration imu_integration(init_bias);

    State state;  
    memset(&state, 0.0, sizeof(state));
    std::vector<Eigen::Vector3d> pose_gt = pose_data;
    double linear_vel = 0.5;
    double true_vx = linear_vel * std::cos(pose_gt[0][2]);
    double true_vy = linear_vel * std::sin(pose_gt[0][2]);
    // state.p = Eigen::Vector2d(gps_data[0][1], gps_data[0][2]);
    state.v = Eigen::Vector2d(true_vx,true_vy);
    // state.theta = 0.001;
    state.ba = Eigen::Vector2d(1e-6,1e-6);
    state.bg = init_bias;

    State last_state = state; 

    Optimizer optimizer;
    bool is_using_ceres = false;
	for(unsigned int i = 0; i < timestamps.size()-1; i++) {

		cout << " \ncurrent fusion idx is ==== : " << i << std::endl;

		const Eigen::Vector3d &last_imu = imu_data[i];
		const Eigen::Vector3d &curr_imu = imu_data[i+1];

		// imu state and cov update
		imu_integration.Integrate(true, last_imu, curr_imu, timestamps[i+1]-timestamps[i], state);

		Eigen::Vector3d last_odometry = odometry_data[i];
		Eigen::Vector3d curr_odometry = odometry_data[i+1];
		Eigen::Vector3d delta_odometry = curr_odometry - last_odometry;

		Eigen::Vector3d last_velocity = vel_data[i];
		Eigen::Vector3d curr_velocity = vel_data[i+1];
		
		Eigen::Vector3d last_pose = pose_data[i];
		Eigen::Vector3d curr_pose = pose_data[i+1];

		optimizer.OptimizeLMCh6(last_state, state, last_velocity, curr_velocity, delta_odometry, last_pose, curr_pose, last_imu, curr_imu, timestamps[i+1]-timestamps[i]);

		cout << "curr_odometry ==== : " << curr_odometry.transpose() << std::endl;
		cout << "delta_odometry ==== : " << delta_odometry.transpose() << std::endl;
		cout << "curr_velocity ==== : " << curr_velocity.transpose() << std::endl;
		cout << "gt_pose ==== : " << pose_data[i+1].transpose() << std::endl;

		std::cout << "optimize state.theta ============= : " << state.theta << std::endl;
    	std::cout << "optimize state.p ============= : " << state.p.transpose() << std::endl;
    	std::cout << "optimize state.v ============= : " << state.v.transpose() << std::endl;
    	std::cout << "optimize state.ba ============= : " << state.ba.transpose() << std::endl;
    	std::cout << "optimize state.bg ============= : " << state.bg << std::endl;
    	// std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;
    	

		last_state = state;

		usleep(150000);
	}

	return 0;
}
